﻿using MySoftLab.DapFlashCore.Probes.DapAccess.Interfaces;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace MySoftLab.DapFlashCore.Probes.DapAccess
{
    /// <summary>
    /// This class implements the CMSIS-DAP wire protocol.
    /// </summary>
    public class CMSISDAPProtocol
    {
        private readonly DapInterface _interface;

        public const byte DAP_OK = 0;
        public const byte DAP_ERROR = 0xff;

        DAPAccessIntf.ID[] INTEGER_INFOS =
        [
                DAPAccessIntf.ID.CAPABILITIES,
                DAPAccessIntf.ID.TEST_DOMAIN_TIMER,
                DAPAccessIntf.ID.SWO_BUFFER_SIZE,
                DAPAccessIntf.ID.MAX_PACKET_COUNT,
                DAPAccessIntf.ID.MAX_PACKET_SIZE
        ];

        public CMSISDAPProtocol(DapInterface @interface)
        {
            _interface = @interface;
        }

        /// <summary>
        /// Sends the DAP_Info command to read info from the CMSIS-DAP probe.
        /// </summary>
        /// <param name="id">id_ One of the @ref pyocd.probe.pydapaccess.dap_access_api.DAPAcessIntf.ID "DAPAcessIntf.ID" constants.</param>
        /// <returns>The `id_` parameter determines the return value data type. For those IDs defined as integer values
        ///    in the DAP_Info command documentation, an int is returned.Otherwise either a string is returned, or
        ///    None if the returned info value length is 0.</returns>
        public string? DapInfo(DAPAccessIntf.ID id)
        {
            List<byte> cmd = [];
            cmd.Add(CommandConsts.DAP_INFO);
            cmd.Add((byte)id);
            _interface.Write(cmd.ToArray());

            var resp = _interface.Read();
            if (resp[0] != CommandConsts.DAP_INFO)
            {
                throw new DAPAccessIntf.DeviceError("expected DAP_INFO");
            }

            var respLen = resp[1];

            // 数字
            if (INTEGER_INFOS.Contains(id))
            {
                if (respLen == 1)
                {
                    return resp[2].ToString();
                }
                else if (respLen == 2)
                {
                    return (resp[3] << 8 | resp[2]).ToString();
                }
                else if (respLen == 4)
                {
                    return (resp[5] << 24 | resp[4] << 16 | resp[3] << 8 | resp[2]).ToString();
                }
                else
                {
                    throw new DAPAccessIntf.DeviceError($"invalid DAP_INFO response length for {id}");
                }
            }

            // String values. They are sent as C strings with a terminating null char, so we strip it out.
            if (respLen == 0)
            {
                return null;
            }

            if (respLen > resp.Length - 2)
            {
                throw new DAPAccessIntf.DeviceError($"invalid DAP_INFO response length for {id}");
            }

            return Encoding.UTF8.GetString(resp.Skip(2).Take(respLen - 1).ToArray());
        }

        public byte SwoMode(byte mode)
        {
            List<byte> cmd = [];

            cmd.Add(CommandConsts.DAP_SWO_MODE);
            cmd.Add(mode);
            _interface.Write(cmd.ToArray());

            var resp = _interface.Read();
            if (resp[0] != CommandConsts.DAP_SWO_MODE)
            {
                throw new DAPAccessIntf.DeviceError("expected DAP_SWO_MODE");
            }

            if (resp[1] != DAP_OK)
            {
                throw new DAPAccessIntf.DeviceError("DAP_SWO_MODE failed");
            }

            return resp[1];
        }

        public byte SwoTransport(byte transport)
        {
            List<byte> cmd = [];

            cmd.Add(CommandConsts.DAP_SWO_TRANSPORT);
            cmd.Add(transport);
            _interface.Write(cmd.ToArray());
            var resp = _interface.Read();

            if (resp[0] != CommandConsts.DAP_SWO_TRANSPORT)
            {
                throw new DAPAccessIntf.DeviceError("expected DAP_SWO_TRANSPORT");
            }

            if (resp[1] != DAP_OK)
            {
                throw new DAPAccessIntf.DeviceError("DAP_SWO_TRANSPORT failed");
            }

            return resp[1];
        }

        public uint SwoBaudrate(uint baudrate)
        {
            List<byte> cmd = [];

            cmd.Add(CommandConsts.DAP_SWO_BAUDRATE);
            cmd.Add((byte)(baudrate & 0xff));
            cmd.Add((byte)(baudrate >> 8 & 0xff));
            cmd.Add((byte)(baudrate >> 16 & 0xff));
            cmd.Add((byte)(baudrate >> 24 & 0xff));

            _interface.Write(cmd.ToArray());

            var resp = _interface.Read();
            if (resp[0] != CommandConsts.DAP_SWO_BAUDRATE)
            {
                throw new DAPAccessIntf.DeviceError("expected DAP_SWO_BAUDRATE");
            }

            return (uint)(resp[1] << 0 | resp[2] << 8 | resp[3] << 16 | resp[4] << 24);
        }

        public byte SwoControl(byte action)
        {
            List<byte> cmd = [];

            cmd.Add(CommandConsts.DAP_SWO_CONTROL);
            cmd.Add(action);

            _interface.Write(cmd.ToArray());

            var resp = _interface.Read();
            if (resp[0] != CommandConsts.DAP_SWO_CONTROL)
            {
                throw new DAPAccessIntf.DeviceError("expected DAP_SWO_CONTROL");
            }

            if (resp[1] != DAP_OK)
            {
                throw new DAPAccessIntf.DeviceError("DAP_SWO_CONTROL failed");
            }

            return resp[1];
        }

        public (byte, uint) SwoStatus()
        {
            List<byte> cmd = [];
            cmd.Add(CommandConsts.DAP_SWO_STATUS);
            _interface.Write(cmd.ToArray());

            var resp = _interface.Read();

            if (resp[0] != CommandConsts.DAP_SWO_STATUS)
            {
                throw new DAPAccessIntf.DeviceError("expected DAP_SWO_STATUS");
            }

            return (resp[1], (uint)(resp[2] << 0 | resp[3] << 8 | resp[4] << 16 | resp[5] << 24));
        }

        public byte SetSwjPins(byte output, byte pins, byte wait = 0)
        {
            List<byte> cmd = [];
            cmd.Add(CommandConsts.DAP_SWJ_PINS);
            cmd.Append(output);
            cmd.Add(pins);
            cmd.Add(wait);
            _interface.Write(cmd.ToArray());

            var resp = _interface.Read();
            if (resp[0] != CommandConsts.DAP_SWJ_PINS)
            {
                throw new DAPAccessIntf.DeviceError("expected DAP_SWJ_PINS");
            }

            return resp[1];
        }

        public byte SetSwjClock(int clock = 1000000)
        {
            List<byte> cmd = [];
            cmd.Add(CommandConsts.DAP_SWJ_CLOCK);
            cmd.Add((byte)(clock & 0xff));
            cmd.Add((byte)(clock >> 8));
            cmd.Add((byte)(clock >> 16));
            cmd.Add((byte)(clock >> 24));
            _interface.Write(cmd.ToArray());

            var resp = _interface.Read();
            if (resp[0] != CommandConsts.DAP_SWJ_CLOCK)
            {
                throw new DAPAccessIntf.DeviceError("expected DAP_SWJ_CLOCK");
            }

            if (resp[1] != DAP_OK)
            {
                throw new DAPAccessIntf.DeviceError("DAP_SWJ_CLOCK failed");
            }

            return resp[1];
        }

        public byte[] Vendor(byte index, byte[] data)
        {
            List<byte> cmd = [];

            cmd.Add((byte)(CommandConsts.DAP_VENDOR0 + index));
            cmd.AddRange(data);
            _interface.Write(cmd.ToArray());

            var resp = _interface.Read();

            if (resp[0] != CommandConsts.DAP_VENDOR0 + index)
            {
                throw new DAPAccessIntf.DeviceError($"expected DAP_VENDOR {index}");
            }

            return resp.Skip(1).ToArray();
        }

        public byte Connect(byte mode = 0)
        {
            List<byte> cmd = [];
            cmd.Add(CommandConsts.DAP_CONNECT);
            cmd.Add(mode);
            _interface.Write(cmd.ToArray());

            var resp = _interface.Read();
            if (resp[0] != CommandConsts.DAP_CONNECT)
            {
                throw new DAPAccessIntf.DeviceError("expected DAP_CONNECT");
            }

            if (resp[1] == 0)
            {
                throw new DAPAccessIntf.DeviceError("DAP_CONNECT failed");
            }

            return resp[1];
        }

        public byte TransferConfigure(byte idleCycles = 0x02, ushort waitRetry = 0x0050, ushort matchRetry = 0)
        {
            List<byte> cmd = [];
            cmd.Add(CommandConsts.DAP_TRANSFER_CONFIGURE);
            cmd.Add(idleCycles);
            cmd.Add((byte)waitRetry);
            cmd.Add((byte)(waitRetry >> 8));
            cmd.Add((byte)matchRetry);
            cmd.Add((byte)(matchRetry >> 8));
            _interface.Write(cmd.ToArray());

            var resp = _interface.Read();
            if (resp[0] != CommandConsts.DAP_TRANSFER_CONFIGURE)
            {
                throw new DAPAccessIntf.DeviceError("expected DAP_TRANSFER_CONFIGURE");
            }

            if (resp[1] != DAP_OK)
            {
                throw new DAPAccessIntf.DeviceError("DAP_TRANSFER_CONFIGURE failed");
            }

            return resp[1];
        }

        public byte SwdConfigure(byte turnAround = 1, bool alwaysSendDataPhase = false)
        {
            if (!(turnAround <= 4 && turnAround >= 1))
            {
                throw new ArgumentException();
            }

            byte otherData = (byte)((alwaysSendDataPhase ? 1 : 0) << 2);
            var conf = (byte)(turnAround - 1 | otherData);

            List<byte> cmd = [];
            cmd.Add(CommandConsts.DAP_SWD_CONFIGURE);
            cmd.Add(conf);
            _interface.Write(cmd.ToArray());

            var resp = _interface.Read();

            if (resp[0] != CommandConsts.DAP_SWD_CONFIGURE)
            {
                throw new DAPAccessIntf.DeviceError("expected DAP_SWD_CONFIGURE");
            }

            if (resp[1] != DAP_OK)
            {
                throw new DAPAccessIntf.DeviceError("DAP_SWD_CONFIGURE failed");
            }

            return resp[1];
        }

        public byte[] JtagConfigure(byte[]? devicesIrLen = null)
        {
            if (null != devicesIrLen)
            {
                devicesIrLen = [4];
            }

            List<byte> cmd = [];

            cmd.Add(CommandConsts.DAP_JTAG_CONFIGURE);
            cmd.Add((byte)devicesIrLen!.Length);
            foreach (byte b in devicesIrLen)
            {
                cmd.Add(b);
            }

            _interface.Write(cmd.ToArray());
            var resp = _interface.Read();

            if (resp[0] != CommandConsts.DAP_JTAG_CONFIGURE)
            {
                throw new DAPAccessIntf.DeviceError("expected DAP_JTAG_CONFIGURE");
            }

            if (resp[1] != DAP_OK)
            {
                throw new DAPAccessIntf.DeviceError("DAP_JTAG_CONFIGURE failed");
            }

            return resp.Skip(2).ToArray();
        }

        public byte SetLed(byte type, byte enabled)
        {
            List<byte> cmd = [];

            cmd.Add(CommandConsts.DAP_LED);
            cmd.Add(type);
            cmd.Add(enabled);
            _interface.Write(cmd.ToArray());

            var resp = _interface.Read();
            if (resp[0] != CommandConsts.DAP_LED)
            {
                throw new DAPAccessIntf.DeviceError("expected DAP_LED");
            }

            if (resp[1] != 0)
            {
                throw new DAPAccessIntf.DeviceError("DAP_LED failed");
            }

            return resp[1];
        }

        public byte SwjSequence(int length, long bits)
        {
            if (!(length <= 256 && length >= 0))
            {
                throw new ArgumentException();
            }

            List<byte> cmd = [];
            cmd.Add(CommandConsts.DAP_SWJ_SEQUENCE);
            cmd.Add(length == 256 ? (byte)0 : (byte)length);

            for (int i = 0; i < (length + 7) / 8; i++)
            {
                cmd.Add((byte)(bits & 0xff));
                bits >>= 8;
            }
            _interface.Write(cmd.ToArray());

            var resp = _interface.Read();
            if (resp[0] != CommandConsts.DAP_SWJ_SEQUENCE)
            {
                throw new DAPAccessIntf.DeviceError("expected DAP_SWJ_SEQUENCE");
            }

            if (resp[1] != DAP_OK)
            {
                throw new DAPAccessIntf.DeviceError("DAP_SWJ_SEQUENCE failed");
            }

            return resp[1];
        }

        /// <summary>
        /// Send the DAP_SWD_Sequence command.
        /// Each sequence in the _sequences_ parameter is a tuple with 1 or 2 members:
        ///- 0: int: number of TCK cycles from 1-64
        ///- 1: int: the SWDIO bit values to transfer.The presence of this tuple member indicates the sequence is
        ///an output sequence; the absence means that the specified number of TCK cycles of SWDIO data will be
        ///read and returned.

        ///The DAP_SWD_Sequence command expects this data for each sequence:
        ///- 0: sequence info byte
        ///    - bit[7]: mode, 0=output, 1=input
        ///    - bit[6]: reserved
        ///    - bits[5:0]: number of TCK cycles from 1-64, with 64 encoded as 0
        ///- 1: (only present if element 0 bit 7 == 0, for output mode) bytes of data to send, one bit per TCK cycle,
        ///transmitted LSB first.

        ///@param self
        ///@param sequences A sequence of sequence description tuples as described above.
        /// </summary>
        /// <param name="sequences">A 2-tuple of the response status, and a sequence of bytes objects, one for each input
        /// sequence.The length of the bytes object is (<TCK-count> + 7) / 8. Bits are in LSB first order.</param>
        /// <returns></returns>
        public (byte, int[]) SwdSequence(List<byte[]> sequences)
        {
            List<byte> cmd = [];

            cmd.Add(CommandConsts.DAP_SWD_SEQUENCE);
            cmd.Add((byte)sequences.Count);
            foreach (var seq in sequences)
            {
                // Construct the control byte.
                byte tckCount = seq[0];

                if (tckCount < 1 || tckCount > 64)
                {
                    throw new ArgumentException("SWD sequence TCK count is out of range (must be 1-64)");
                }
                bool isOutput = seq.Length == 2;
                byte info = isOutput ? (byte)0 : (byte)0x80;

                if (tckCount != 64)
                {
                    info |= tckCount;
                }

                cmd.Add(info);
                // Append SWDIO output data.

                if (isOutput)
                {
                    var bits = seq[1];

                    for (int i = 0; i < (tckCount + 7) / 8; i++)
                    {
                        cmd.Add((byte)(bits & 0xff));
                        bits >>= 8;
                    }
                }
            }

            _interface.Write(cmd.ToArray());

            var resp = _interface.Read();
            if (resp[0] != CommandConsts.DAP_SWD_SEQUENCE)
            {
                throw new DAPAccessIntf.DeviceError("expected DAP_SWD_SEQUENCE");
            }

            if (resp[1] != DAP_OK)
            {
                throw new DAPAccessIntf.DeviceError("DAP_SWD_SEQUENCE failed");
            }

            List<int> result = [];

            int offset = 2;
            foreach (var seq in sequences)
            {
                // Only handle input sequences. Convert the bytes into an int, LSB first.
                if (seq.Length == 1)
                {
                    var tckCount = seq[0];
                    var byteCount = (tckCount + 7) / 8;
                    int bits = 0;
                    for (int i = 0; i < byteCount; i++)
                    {
                        bits |= resp[offset + i] << 8 * i;
                    }
                    offset += byteCount;
                    result.Add(bits);
                }
            }

            return (resp[1], result.ToArray());
        }

        public byte JtagSequence(byte cycles, byte tms, byte readTdo, int tdi)
        {
            if (cycles > 64)
            {
                throw new ArgumentException();
            }

            byte info = cycles == 64 ? (byte)0 : cycles;
            info &= 0x3f;
            info |= (byte)((tms & 1) << 6);
            info |= (byte)(readTdo << 7);

            List<byte> cmd = [];
            cmd.Add(CommandConsts.DAP_JTAG_SEQUENCE);
            cmd.Add(1);
            cmd.Add(info);

            for (int i = 0; i < (cycles + 7) / 8; i++)
            {
                cmd.Append((byte)(tdi & 0xff));
                tdi >>= 8;
            }

            _interface.Write(cmd.ToArray());

            var resp = _interface.Read();
            if (resp[0] != CommandConsts.DAP_JTAG_SEQUENCE)
            {
                throw new DAPAccessIntf.DeviceError("exected DAP_JTAG_SEQUENCE");
            }

            if (resp[1] != DAP_OK)
            {
                throw new DAPAccessIntf.DeviceError("DAP_JTAG_SEQUENCE failed");
            }

            return resp[2];
        }

        public byte Disconnect()
        {
            List<byte> cmd = [];

            cmd.Add(CommandConsts.DAP_DISCONNECT);
            _interface.Write(cmd.ToArray());

            var resp = _interface.Read();
            if (resp[0] != CommandConsts.DAP_DISCONNECT)
            {
                throw new DAPAccessIntf.DeviceError("expected DAP_DISCONNECT");
            }

            if (resp[1] != DAP_OK)
            {
                throw new DAPAccessIntf.DeviceError("DAP_DISCONNECT failed");
            }

            return resp[1];
        }
    }

    /// <summary>
    /// SWO mode options.
    /// </summary>
    public static class DAPSWOMode
    {
        public const byte OFF = 0;
        public const byte UART = 1;
        public const byte MANCHESTER = 2;
    }

    /// <summary>
    /// SWO control actions.
    /// </summary>
    public static class DAPSWOControl
    {
        public const byte STOP = 0;
        public const byte START = 1;
    }

    /// <summary>
    /// Options for DAP_SWO_TRANSPORT command.
    /// </summary>
    public static class DAPSWOTransport
    {
        public const byte NONE = 0;
        public const byte DAP_SWO_DATA = 1;
        public const byte DAP_SWO_EP = 2;
    }

    public static class Capabilities
    {
        public const byte SWD = 0x01;
        public const byte JTAG = 0x02;
        public const byte SWO_UART = 0x04;
        public const byte SWO_MANCHESTER = 0x08;
        public const byte ATOMIC_COMMANDS = 0x10;
        public const byte DAP_SWD_SEQUENCE = 0x20;
    }

    public static class Pin
    {
        public const byte NONE = 0x00; // Used to read current pin values without changing.
        public const byte SWCLK_TCK = 1 << 0;
        public const byte SWDIO_TMS = 1 << 1;
        public const byte TDI = 1 << 2;
        public const byte TDO = 1 << 3;
        public const byte nTRST = 1 << 5;
        public const byte nRESET = 1 << 7;
    }

    public static class CommandConsts
    {
        public const byte DAP_INFO = 0x00;
        public const byte DAP_LED = 0x01;
        public const byte DAP_CONNECT = 0x02;
        public const byte DAP_DISCONNECT = 0x03;
        public const byte DAP_TRANSFER_CONFIGURE = 0x04;
        public const byte DAP_TRANSFER = 0x05;
        public const byte DAP_TRANSFER_BLOCK = 0x06;
        public const byte DAP_TRANSFER_ABORT = 0x07;
        public const byte DAP_WRITE_ABORT = 0x08;
        public const byte DAP_DELAY = 0x09;
        public const byte DAP_RESET_TARGET = 0x0a;
        public const byte DAP_SWJ_PINS = 0x10;
        public const byte DAP_SWJ_CLOCK = 0x11;
        public const byte DAP_SWJ_SEQUENCE = 0x12;
        public const byte DAP_SWD_CONFIGURE = 0x13;
        public const byte DAP_JTAG_SEQUENCE = 0x14;
        public const byte DAP_JTAG_CONFIGURE = 0x15;
        public const byte DAP_JTAG_IDCODE = 0x16;
        public const byte DAP_SWO_TRANSPORT = 0x17;
        public const byte DAP_SWO_MODE = 0x18;
        public const byte DAP_SWO_BAUDRATE = 0x19;
        public const byte DAP_SWO_CONTROL = 0x1A;
        public const byte DAP_SWO_STATUS = 0x1B;
        public const byte DAP_SWO_DATA = 0x1C;
        public const byte DAP_SWD_SEQUENCE = 0x1D;
        public const byte DAP_SWO_EXTENDED_STATUS = 0x1E;
        public const byte DAP_QUEUE_COMMANDS = 0x7E;
        public const byte DAP_EXECUTE_COMMANDS = 0x7F;
        public const byte DAP_VENDOR0 = 0x80; // Start of vendor-specific command IDs.
    }

    /// <summary>
    /// Responses to DAP_Transfer and DAP_TransferBlock
    /// </summary>
    public static class DAPTransferResponse
    {
        public const byte ACK_MASK = 0x07; // Bits [2:0]
        public const byte PROTOCOL_ERROR_MASK = 0x08; // Bit [3]
        public const byte VALUE_MISMATCH_MASK = 0x08; // Bit [4]

        // Values for ACK bitfield.
        public const byte ACK_OK = 1;
        public const byte ACK_WAIT = 2;
        public const byte ACK_FAULT = 4;
        public const byte ACK_NO_ACK = 7;
    }

    public static class CMSISDAPVersion
    {
        public static readonly (int, int, int) V1_0_0 = (1, 0, 0);
        public static readonly (int, int, int) V1_1_0 = (1, 1, 0);
        public static readonly (int, int, int) V1_2_0 = (1, 2, 0);
        public static readonly (int, int, int) V1_3_0 = (1, 3, 0);
        public static readonly (int, int, int) V2_0_0 = (2, 0, 0);
        public static readonly (int, int, int) V2_1_0 = (2, 1, 0);

        public static HashSet<int> MajorVersions()
        {
            return new HashSet<int>()
            {
                1, 2
            };
        }
    }
}
